#include "Leg.h"
#include "Global.h"
#include "myMath.h"

#define KPX 0.0003f
#define KDX 0.0009f
#define KPZ 0.0003f
#define KDZ 0.0009f

void Leg_Init(Leg_t *leg, uint8_t legID, HT_t *motorF, HT_t *motorB)
{
    leg->ID = legID;
    leg->motorF = motorF;
    leg->motorB = motorB;

    // leg->heigth = 300; // 小于(最长腿长)405
    // leg->H = 60; // 小于(高度-最小腿长)330-165=165
    // leg->S = 0;  // 小于(sqrt(最长腿长^2-高度^2))sqrt(405^2 - 330^2)=272
}

void Leg_Run(Leg_t *leg)
{
#if OPEN_CAN
    /* 电机运行 */
    HT_Run(leg->motorF);
    HT_Run(leg->motorB);
#endif
}

/* 根据当前相位计算摆线轨迹 */
void calcCycloid(Leg_t *leg)
{
    float p, x, z;

    /* 计算复合摆线x, z */
    p = _2PI * global.phase[leg->ID];
    if (global.contact[leg->ID]) /* 足端触地时 */
    {
        x = leg->S * (sinf(p) / _2PI - global.phase[leg->ID] + 0.5f);
        z = -global.height;
    }
    else /* 足端摆动 */
    {
        x = leg->S * (global.phase[leg->ID] - sinf(p) / (_2PI)-0.5f);

        if (global.phase[leg->ID] < 0.5f)
        {
            z = 2 * leg->H * (global.phase[leg->ID] - sinf(2 * p) / _4PI) - global.height;
        }
        else
        {
            z = 2 * leg->H * (1 - global.phase[leg->ID] + sinf(2 * p) / (_4PI)) - global.height;
        }
    }

    /* 运动学逆解 */
    state_inv_solution(&leg->vmc, x, z, &leg->alpha, &leg->beta);

#if USE_FORCE
    float Vx, Vz;
    float currentX, currentZ, currentVx, currentVz;

    if (global.contact[leg->ID]) /* 足端触地时 */
    {

        Vx = leg->S * (cosf(p) - 1) / global.throttle / 1000.0f;
        Vz = 0;
    }
    else /* 足端摆动 */
    {
        Vx = leg->S * (1 - cosf(p)) / global.throttle / 1000.0f;

        if (global.phase[leg->ID] < 0.5f)
        {
            Vz = 2 * leg->H * (1 - cosf(2 * p)) / global.throttle / 1000.0f;
        }
        else
        {
            Vz = 2 * leg->H * (cosf(2 * p) - 1) / global.throttle / 1000.0f;
        }
    }

    /* 运动学正解 */
    state_solution(&leg->vmc, -leg->motorF->Position, _PI - leg->motorB->Position, &currentX, &currentZ);
    /* 计算雅可比矩阵 */
    cal_jacobi(&leg->vmc);
    /* 计算逆矩阵 */
    cal_invJacobi(&leg->vmc);

    /* 计算电机预期速度 */
    cal_omega(&leg->vmc, Vx, Vz, &leg->alphaW, &leg->betaW);
    /* 计算足端实际速度 */
    cal_velcity(&leg->vmc, leg->alphaW, leg->betaW, &currentVx, &currentVz);

    /* 计算修正力 */
    float forceX = KPX * (x - currentX) + KDX * (Vx - currentVx);
    float forceZ = KPZ * (z - currentZ) + KDX * (Vz - currentVz);
    /* 计算电机扭矩 */
    cal_torque(&leg->vmc, forceX, forceZ, &leg->correntForceF, &leg->correntForceB);
#endif /* USE_FORCE */
}
